//----------------------------------------------------------------------------
// Filename: 
//----------------------------------------------------------------------------
#include <p18F4520.h>
#include <adc.h>
#include <delays.h>
#include "servo.h"
#include "ieee_robot.h"
#include "motor.h"


#pragma config OSC = EC //this sets the oscillator to external clock on
//OSC1 pin (pin 13). The PICDEM 2 plus board
//uses a 4 MHz external clock, other configs
//should use different setting

/*------------------Global Variables for Interrupt Communication------------------*/
unsigned short int servo_position = SERVO_STRAIGHT;
//unsigned short int servo_position =  SERVO_LEFT_45;
unsigned short int a2d_result = 0;        //variable for storing A2D results
unsigned short int SLOW_MOTOR_FLAG = 0;	//use this flag to know when to
							//slow motor
//----------------------------------------------------------------------------

/*-------------------Local Functions----------------------------------------------*/

void delay_ms(unsigned char delay) {
    #undef MULT //undef any previous defines of MULT

#if TCYCNT_4_MS >= 10000
    #if TCYCNT_4_MS > 10000
        #define MULT TCYCNT_4_MS / 10000
    #else
        #define MULT 1
    #endif
        Delay10KTCYx(MULT*delay);
#elif TCYCNT_4_MS >= 1000
    #if TCYCNT_4_MS > 1000
        #define MULT TCYCNT_4_MS / 1000
    #else
        #define MULT 1
    #endif
        Delay1KTCYx(MULT*delay);
#elif TCYCNT_4_MS >= 100
    #if TCYCNT_4_MS > 100
        #define MULT TCYCNT_4_MS / 100
    #else
        #define MULT 1
    #endif
        Delay100TCYx(MULT*delay);
#elif TCYCNT_4_MS >= 10
    #if TCYCNT_4_MS > 10
        #define MULT TCYCNT_4_MS / 10
    #else
        #define MULT 1
    #endif
        Delay10TCYx(MULT*delay);
#elif TCYCNT_4_MS < 10
        unsigned char i;
        for (i = 0; i < TCYCNT_4_MS;i++) {
            _asm
                nop
            _endasm
        }
        return;
#else
    #warning "Something went wrong in delay_ms, IEEE_Robot_Main.c" 
#endif
}


/*-------------------Timer 3 operation functions and variables--------------------*/


//----------------------------------------------------------------------------------
// Main routine

void main (void) {
/*-------------------------ADC SETUP AND CONFIGURATION-----------------------------*/
     //Initialize ADC so that clock is fosc/4, result stored in least significant
     //bits of regester, acuisition time set to 12 TAD, AN0 is input channel, 
     //interrupts are on, refrences points are VDD and VSS, and AN<0:3> are 
     //analog signal while rest are digital
     OpenADC( ADC_FOSC_64 & ADC_RIGHT_JUST & ADC_12_TAD, ADC_CH1 & ADC_INT_ON & ADC_REF_VDD_VSS, 13 );		
     IPR1 = IPR1 & (~0x40); //set A2D as low priority interrupts
     //init PORT A
     PORTA = 0;
     TRISA = 0xFB; //port a all inputs, except RA2
     ConvertADC();				  //initial ADC 

/* -------Enable USART ------------------------------------------------------------*/

     
/*--------Timer 0 and Timer 1 setup and interrupt configuration--------------------*/
     INTCON = 0x20;                //disable global and enable TMR0 interrupt
                                  //while clearing TMR0IF flag bit3
     RCONbits.IPEN = 1;            //enable interrupt priority levels
     init_usart();

     /*INTCON2 = 0x84;               //TMR0 high priority                            

     IPR1 = IPR1 | 0x01;           //set Timer 1 interrupts high priority
     PIR1 = PIR1 & (~0x01);         //clear TMR1F interrupt flag
     PIE1 = PIE1 & (~0x01);        //disable Timer 1 interrupts
     write_timer0(TMR0_VALUE);     //write to Timer 0 the value determine by 
                                  //experimentation to provide 50 Hz interrupts
     T0CON = 0x88;  //set up timer0 - prescaler 1:1, with 16
                   //bit selected

     T1CON = 0x91;  //setup TICON for TMR1 to use Fosc/4 with 1:2 prescaler
                   //with 16 bit rights and read of TMR1H:TMR1:L
     */

     //servo_position = SERVO_LEFT_45;  
     //enable global and low priority interrupts              
     INTCON = INTCON | 0xC0; 
/*-----------------END SERVO INIT-------------------------------------------------*/
    

/*-----------------------------PWM INIT-------------------------------------------*/
     //initialize the PWM signal for motor driver IC
     //note 100 = 50% duty cycle
     init_ccp1();
     set_duty_ccp1(195);
     start_ccp1();
    
     //PIE1 = PIE1 & (~0x06); //clear CCP1E and TMR2IE
/*----------------------------END PWM INIT---------------------------------------*/



/*---------------------------SET UP WHISKER SENSORS--------------------------------*/
	 //init PORT B
	 //Inputs:  INT1 (bumperL), INT2 (bumperR), KBI0(front), KBI1(mid), KBI2(rear)
	 //Outputs:  RB0, RB3, RB7  (can be used for debuggin)
      TRISB = 0x76;				  
	INTCONbits.RBIE = 1;     	  //enable port change interrupts 
	INTCON2 = INTCON2 & 0xEE;	  //set INTEDGE2 to falling edge set whiskers to low priority
								  //this is for front whisker testing -> start with bot on wall
	 //INTCON2 = INTCON2 | 0x10	  //re-implement for bumper: INTEDGE2 should be rising edge
      INTCON3 = INTCON3 & 0x37;     //set bumper priority to low, turn off bumperL until whisker 
								  //debugging done
      INTCON3bits.INT2IE = 1;		  //enable INT1
	 //INTCON3 = INCON3 | 0x18;    //enable INT1 and INT2 for bumpers


/*--------------------------END WHISKER SENSORS------------------------------------*/
     //unsigned char y = 0;

     while (1) {
          //ADCON0bits.ADON = ~ADCON0bits.ADON;
          //delay_ms(100);
          set_position(250);
		  
          delay_ms(5000);
     }
}




